#include "captureimg.h"
#include "userconfig.h"

int captureimg_Clock(uintptr_t wObjectAddr);
int captureimg_Run(uintptr_t wObjectAddr);

extern ggpio_t c_tTrigger[3];
extern ggpio_t c_tInclusive[3];
extern stepmotor_t c_tStepMotor[3];

uint8_t forward[8] = {0x08, 0x0c, 0x04, 0x06, 0x02, 0x03, 0x01, 0x09};
uint8_t invert[8] = {0x09, 0x01, 0x03, 0x02, 0x06, 0x04, 0x0c, 0x08};
// hardware
void stepmotor_Control(uint8_t chStepMotorNum, uint8_t chInputData)
{
    STEPMOTOR_A(chStepMotorNum, chInputData&0x08);
    STEPMOTOR_B(chStepMotorNum, chInputData&0x04);
    STEPMOTOR_C(chStepMotorNum, chInputData&0x02);
    STEPMOTOR_D(chStepMotorNum, chInputData&0x01);
}

// running in 1ms clock
void captureimg_StepMotorWork(captureimg_t *ptThis, uint8_t chStepMotorNum, uint8_t chDirection)
{
    uint16_t hwIndex = 0;
    static uint8_t s_chIndex = 0;
    uint8_t chInputData = 0;
    uint8_t *pchStepData = NULL;
    static uint8_t s_chSpeed[3] = {2,2,2};
    if(s_chSpeed[chStepMotorNum])
    {
        s_chSpeed[chStepMotorNum]--;
        return;
    }
    else
    {
        s_chSpeed[chStepMotorNum] = 2;
    }
        
    
    if(ptThis->chStepMotorFlag[chStepMotorNum])
    {
        if(chDirection == 0)
        {
            pchStepData = forward;
        }
        else
        {
            pchStepData = invert;
        }
        chInputData = pchStepData[s_chIndex];
        stepmotor_Control(chStepMotorNum, chInputData);
        if(s_chIndex < 7)
        {
            s_chIndex++;
        }
        else
        {
            s_chIndex = 0;
        }
    }
    else
    {
        stepmotor_Control(chStepMotorNum, 0);
    }

}

void captureimg_StepMotorStop(captureimg_t *ptThis, uint8_t chStepMotorNum)
{
    ptThis->chStepMotorFlag[chStepMotorNum] = 0;
}

void captureimg_StepMotorStart(captureimg_t *ptThis, uint8_t chStepMotorNum)
{
    ptThis->chStepMotorFlag[chStepMotorNum] = 1;
}

void captureimg_InclusiveWork(captureimg_t *ptThis, uint8_t chInclusiveNum)
{
    static uint8_t s_chInclusiveCount[3] = {0,0,0};
    // T is 30ms;Ton is 3ms;Toff is 27ms
    if(ptThis->chInclusiveTimeFlag[chInclusiveNum])
    {
        if(s_chInclusiveCount[chInclusiveNum] < 30)
        {
            if(s_chInclusiveCount[chInclusiveNum] < 3)
            {
                INCLUSIVE(chInclusiveNum, 1);
            }
            else
            {
                INCLUSIVE(chInclusiveNum, 0);
            }
            s_chInclusiveCount[chInclusiveNum]++;
        }
        else
        {
            s_chInclusiveCount[chInclusiveNum] = 0;
        }
    }
    else
    {
        s_chInclusiveCount[chInclusiveNum] = 0;
    }
}

void captureimg_InclusiveStop(captureimg_t *ptThis, uint8_t chInclusiveNum)
{
    ptThis->chInclusiveTimeFlag[chInclusiveNum] = 0;
    INCLUSIVE(chInclusiveNum, 0);
}

void captureimg_InclusiveStart(captureimg_t *ptThis, uint8_t chInclusiveNum)
{
    ptThis->chInclusiveTimeFlag[chInclusiveNum] = 1;
}

void captureimg_TriggerWork(captureimg_t *ptThis, uint8_t chTriggerNum)
{
    static uint8_t s_chTriggerStatus[5] = {0, 0, 0, 0, 0};
    switch (s_chTriggerStatus[chTriggerNum])
    {
    case 0:
        if(ptThis->chTriggerFlag[chTriggerNum])
        {
            s_chTriggerStatus[chTriggerNum]++;
            TRIGGER(chTriggerNum, 1);
            ptThis->chTriggerFlag[chTriggerNum] = 0;
        }
        break;
    case 1:
        TRIGGER(chTriggerNum, 0);
        s_chTriggerStatus[chTriggerNum] = 0;
        /* code */
        break;
    default:
        break;
    }
}

void captureimg_TriggerStart(captureimg_t *ptThis, uint8_t chTriggerNum)
{
    ptThis->chTriggerFlag[chTriggerNum] = 1;
}

// Define a global captureimg base of type gmsi_base_t
static gmsi_base_t s_tCaptureimgBase;

// Define and initialize a global captureimg base configuration of type gmsi_base_cfg_t
gmsi_base_cfg_t s_tCaptureimgBaseCfg = {
    .wId = CAPTUREIMG,                  // Set the ID to CAPTUREIMG
    .wParent = 0,                       // Set the parent to 0
    .FcnInterface = {
        .Clock = captureimg_Clock,      // Set the Clock function to captureimg_Clock
        .Run = captureimg_Run,          // Set the Run function to captureimg_Run
    },
};

gcoroutine_handle_t tGcoroutineCaptureimgHandle = {
    .bIsRunning = false,
    .pfcn = NULL,
};

#define STEPMOTOR_RUNTIME   100
#define EXPOSURE_RUNTIME    100
#define CAPTUREWAIT_RUNTIME 10
uint16_t g_hwStepMotorRunTime[5] = {80, 70, 90, 70, 50};
uint8_t g_chStepMotorFlag[5] = {0,1,0,1,0};
/**
 * Function: captureimg_gcoroutine
 * ----------------------------
 * This function is a coroutine that handles different states of the captureimg object. It receives 
 * a parameter, converts it to an captureimg object, and uses a switch statement to handle different 
 * states. This function returns fsm_rt_on_going if it is still running, or fsm_rt_cpl if it has 
 * completed its task. Once the coroutine has completed its task, it will be removed from the 
 * coroutine list.
 *
 * Parameters: 
 * pvParam: The parameter to be converted to an captureimg object.
 *
 * Returns: 
 * fsm_rt_on_going if the function is still running, or fsm_rt_cpl if the function has completed 
 * its task.
 */
fsm_rt_t captureimg_gcoroutine(void *pvParam)
{
    static uint8_t s_eState = 0;
    static uint8_t s_chReaptCount = 0;
    fsm_rt_t tFsm = fsm_rt_on_going;
    captureimg_t *ptThis = (captureimg_t *)pvParam;
    uint8_t chOffset = 0;
    // Check if ptThis is not NULL
    if (ptThis == NULL) {
        GLOG_PRINTF("Error: ptThis is NULL.\n");
        return fsm_rt_err;
    }
    ptThis->chRunTimeCount = s_chReaptCount;
    switch(s_eState)
    {
        // start inclusive
        case 0:
            if(!s_chReaptCount)
                GLOG_PRINTF("Start capture");
            for(chOffset = 0; chOffset < 3; chOffset++)
            {
                captureimg_InclusiveStart(ptThis, chOffset);
            }
            ptThis->chCaptureWaitCount = CAPTUREWAIT_RUNTIME;
            s_eState++;
            break;
        case 1:
            if(ptThis->chCaptureWaitCount == 1)
            {
                ptThis->chCaptureWaitCount = 0;
                s_eState++;
            }
            break;
        case 2:
            for(chOffset = 0; chOffset < 3; chOffset++)
            {
                captureimg_TriggerStart(ptThis, chOffset);
            }
            ptThis->chExposureTimeCount = EXPOSURE_RUNTIME;
            s_eState++;
            break;
        case 3:
            if(ptThis->chExposureTimeCount == 1)
            {
                ptThis->chExposureTimeCount = 0;

                for(chOffset = 0; chOffset < 3; chOffset++)
                {
                    captureimg_InclusiveStop(ptThis, chOffset);
                    captureimg_StepMotorStart(ptThis, chOffset);
                    //ptThis->hwStepMotorRunCount = STEPMOTOR_RUNTIME;
                    ptThis->hwStepMotorRunCount = g_hwStepMotorRunTime[ptThis->chRunTimeCount];
                }
                s_eState++;
            }
            break;
        case 4:
            if(ptThis->hwStepMotorRunCount == 1)
            {
                ptThis->hwStepMotorRunCount = 0;
                for(chOffset = 0; chOffset < 3; chOffset++)
                {
                    captureimg_StepMotorStop(ptThis, chOffset);
                }
                ptThis->chCaptureWaitCount = CAPTUREWAIT_RUNTIME;
                s_eState++;
            }
        case 5:
            if(ptThis->chCaptureWaitCount == 1)
            {
                s_chReaptCount++;
                if(s_chReaptCount > 4)
                    s_eState++;
                else
                    s_eState = 0;
            }
            break;
        case 6:
            s_chReaptCount = 0;
            GLOG_PRINTF("Stop capture");
            fsm_cpl();
            break;
        default:
            fsm_cpl();
        break;
    }
    fsm_on_going(); 
}

fsm_rt_t captureimg_gcoroutineOnce(void *pvParam)
{
    static uint8_t s_eState = 0;
    static uint8_t s_chReaptCount = 0;
    fsm_rt_t tFsm = fsm_rt_on_going;
    captureimg_t *ptThis = (captureimg_t *)pvParam;
    uint8_t chOffset = 0;
    // Check if ptThis is not NULL
    if (ptThis == NULL) {
        GLOG_PRINTF("Error: ptThis is NULL.\n");
        return fsm_rt_err;
    }
    ptThis->chRunTimeCount = s_chReaptCount;
    switch(s_eState)
    {
        // start inclusive
        case 0:
            if(!s_chReaptCount)
                GLOG_PRINTF("Start capture");
            for(chOffset = 0; chOffset < 3; chOffset++)
            {
                captureimg_InclusiveStart(ptThis, chOffset);
            }
            ptThis->chCaptureWaitCount = CAPTUREWAIT_RUNTIME;
            s_eState++;
            break;
        case 1:
            if(ptThis->chCaptureWaitCount == 1)
            {
                ptThis->chCaptureWaitCount = 0;
                s_eState++;
            }
            break;
        case 2:
            if(!s_chReaptCount)
            {
                ptThis->chExposureDelayCount = 1;
            }
            else
            {
                ptThis->chExposureDelayCount = ptThis->chExposureDelay[s_chReaptCount-1];
            }
            s_eState++;
            
            break;
        case 3:
            if(ptThis->chExposureDelayCount == 1)
            {
                captureimg_TriggerStart(ptThis, s_chReaptCount);
                s_eState++;
                ptThis->chExposureTimeCount = 0;
                if(s_chReaptCount == 2)
                    ptThis->chWaitReaptcount = 100;
                else
                    ptThis->chWaitReaptcount = 2;
                
            }
            break;
        case 4:
            if(ptThis->chWaitReaptcount == 1)
            {
                if(s_chReaptCount >= 2)
                {
                    GLOG_PRINTF("end capture");
                    //ptThis->hwStepMotorRunCount = 40;
                    for(chOffset = 0; chOffset < 3; chOffset++)
                    {
                        captureimg_InclusiveStop(ptThis, chOffset);
                        //captureimg_StepMotorStart(ptThis, chOffset);
                    }
                    //s_eState++;
                    s_chReaptCount = 0;
                    fsm_cpl();
                }
                else
                {
                    s_chReaptCount++;
                    s_eState = 2;
                }
                ptThis->chWaitReaptcount = 0;
            }
        case 5:
            if(ptThis->hwStepMotorRunCount == 1)
            {
                s_chReaptCount = 0;
                for(chOffset = 0; chOffset < 3; chOffset++)
                {
                    captureimg_StepMotorStop(ptThis, chOffset);
                }
                fsm_cpl();
            }
            break;
        default:
            for(chOffset = 0; chOffset < 3; chOffset++)
            {
                captureimg_InclusiveStop(ptThis, chOffset);
            }
            fsm_cpl();
    }
    fsm_on_going();
}

/**
 * Function: captureimg_EventHandle
 * ----------------------------
 * This function handles the events for the captureimg object. It checks for specific events and 
 * performs the corresponding actions. This function does not return a value, as its result is 
 * reflected in the state of the captureimg object it operates on.
 *
 * Parameters: 
 * ptThis: The pointer to the captureimg object.
 * wEvent: The events to be handled.
 *
 * Returns: 
 * None
 */
static void captureimg_EventHandle(captureimg_t *ptThis, uint32_t wEvent)
{
    uint8_t chOffset = 0;
    // Check if ptThis is not NULL
    if (ptThis == NULL) {
        GLOG_PRINTF("ptThis is NULL.");
        return;
    }

    // Check if the event Event_SyncButtonPushed is set
    if(wEvent & INCLUSIVE_Event_Captureimg)
    {
        GLOG_PRINTF("INCLUSIVE_Event_Captureimg");
        if(GMSI_SUCCESS != gcoroutine_Insert(&tGcoroutineCaptureimgHandle, (void *)ptThis, captureimg_gcoroutine))
        {
            GLOG_PRINTF("Error: gcoroutine_Insert failed.");
        }
    }
    if(wEvent & INCLUSIVE_Event_StartScatteredSpotsAll)
    {
        for(chOffset = 0; chOffset < 3; chOffset++)
        {
            captureimg_InclusiveStart(ptThis, chOffset);
        }
    }   
    if(wEvent & INCLUSIVE_Event_StopScatteredSpotsAll)
    {
        for(chOffset = 0; chOffset < 3; chOffset++)
        {
            captureimg_InclusiveStop(ptThis, chOffset);
        }
    }
    if(wEvent & INCLUSIVE_Event_StartScatteredSpots1)
    {
        captureimg_InclusiveStart(ptThis, 0);
    }
    if(wEvent & INCLUSIVE_Event_StopScatteredSpots1)
    {
        captureimg_InclusiveStop(ptThis, 0);
    }
    if(wEvent & INCLUSIVE_Event_StartScatteredSpots2)
    {
        captureimg_InclusiveStart(ptThis, 1);
    }
    if(wEvent & INCLUSIVE_Event_StopScatteredSpots2)
    {
        captureimg_InclusiveStop(ptThis, 1);
    }
    if(wEvent & INCLUSIVE_Event_StartScatteredSpots3)
    {
        captureimg_InclusiveStart(ptThis, 2);
    }
    if(wEvent & INCLUSIVE_Event_StopScatteredSpots3)
    {
        captureimg_InclusiveStop(ptThis, 2);
    }
    if(wEvent & INCLUSIVE_Event_Trigger1)
    {
        GLOG_PRINTF("Trigger Start 1");
        captureimg_TriggerStart(ptThis, 0);
    }
    if(wEvent & INCLUSIVE_Event_Trigger2)
    {
        GLOG_PRINTF("Trigger Start 2");
        captureimg_TriggerStart(ptThis, 1);
    }
    if(wEvent & INCLUSIVE_Event_Trigger3)
    {
        GLOG_PRINTF("Trigger Start 3");
        captureimg_TriggerStart(ptThis, 2);
    }
    if(wEvent & INCLUSIVE_Event_Trigger4)
    {
        GLOG_PRINTF("Trigger Start 4");
        captureimg_TriggerStart(ptThis, 3);
    }
    if(wEvent & INCLUSIVE_Event_Trigger5)
    {
        GLOG_PRINTF("Trigger Start 5");
        captureimg_TriggerStart(ptThis, 4);
    }
}

/**
 * Function: captureimg_Run
 * ----------------------------
 * This function is mounted in the GMSI run function list and is called within a while(1) loop. 
 * It receives events and messages from other objects. When a corresponding event is triggered, 
 * it executes the appropriate coroutine. The function gets the captureimg object from the given 
 * address, retrieves the events for the object, and handles these events. This function returns 
 * GMSI_SUCCESS if it runs successfully.
 *
 * Parameters: 
 * wObjectAddr: The address of the captureimg object.
 *
 * Returns: 
 * GMSI_SUCCESS if the function runs successfully.
 */
int captureimg_Run(uintptr_t wObjectAddr)
{
    int wRet = GMSI_SUCCESS;
    uint8_t chOffset;
    uint32_t wEvent;
    GMSI_MSG_DECLARE(DataFrame, 10);
    // Get the captureimg object from the given address
    captureimg_t *ptThis = (captureimg_t *)wObjectAddr;

    // Check if ptThis is not NULL
    if (ptThis == NULL) {
        GLOG_PRINTF("ptThis is NULL.");
        return GMSI_EFAIL;
    }

    // Get the events for the captureimg object
    wEvent = gbase_EventPend(ptThis->ptBase);
    // If there are any events, handle them
    if(wEvent)
        captureimg_EventHandle(ptThis, wEvent);
    
    if(gbase_MessagePend(ptThis->ptBase, GMSI_MSG_GET_HANDLE(DataFrame)) > 0)
    {
        GLOG_PRINTF("GET DATA");
        uint8_t* pchData = GMSI_MSG_GET_BUFFER(DataFrame);
        if(GMSI_MSG_GET_LENGTH(DataFrame) == 2)
        {
            ptThis->chExposureDelay[0] = pchData[0];
            ptThis->chExposureDelay[1] = pchData[1];
            if(GMSI_SUCCESS != gcoroutine_Insert(&tGcoroutineCaptureimgHandle, (void *)ptThis, captureimg_gcoroutineOnce))
            {
                GLOG_PRINTF("Error: gcoroutine_Insert failed.");
            }
        }
        else if(GMSI_MSG_GET_LENGTH(DataFrame) == 3)
        {
            // set time
            ptThis->hwStepMotorRunAloneCount[0] = pchData[0];
            ptThis->hwStepMotorRunAloneCount[1] = pchData[1];
            ptThis->hwStepMotorRunAloneCount[2] = pchData[2];
            // Setting direction
            if(g_chStepMotorFlag[0] == 1)
            { 
                g_chStepMotorFlag[0] = 0;
            }
            else
            {
                g_chStepMotorFlag[0] = 1;
            }
            for(chOffset = 0; chOffset < 3; chOffset++)
            {
                captureimg_StepMotorStart(ptThis, chOffset);
            }
        }
        else if(GMSI_MSG_GET_LENGTH(DataFrame) == 6)
        {
            ptThis->hwStepMotorRunAloneCount[0] = pchData[0]*256 + pchData[1];
            ptThis->hwStepMotorRunAloneCount[1] = pchData[2]*256 + pchData[3];
            ptThis->hwStepMotorRunAloneCount[2] = pchData[4]*256 + pchData[5];
            if(ptThis->hwStepMotorRunAloneCount[0] > ptThis->hwStepMotorRunAloneCount[1])
                ptThis->chStepMotorMaxTime = 0;
            else
            {
                if(ptThis->hwStepMotorRunAloneCount[1] > ptThis->hwStepMotorRunAloneCount[2])
                    ptThis->chStepMotorMaxTime = 1;
                else
                    ptThis->chStepMotorMaxTime = 2;
            }
        }
    }
    // Logic or state machine programs

    return wRet;
}

/**
 * Function: captureimg_Clock
 * ----------------------------
 * This function is called in a 1ms interrupt. It gets the captureimg object from the given address 
 * and performs operations on it. This function returns GMSI_SUCCESS if it runs successfully.
 *
 * Parameters: 
 * wObjectAddr: The address of the captureimg object.
 *
 * Returns: 
 * GMSI_SUCCESS if the function runs successfully.
 */
int captureimg_Clock(uintptr_t wObjectAddr)
{
    uint8_t chStepMotorNumOffset = 0;
    static uint8_t s_chTestFlag = 0;
    // Get the captureimg object from the given address
    captureimg_t *ptThis = (captureimg_t *)wObjectAddr;

    int wRet = GMSI_SUCCESS;
    
    if(ptThis->hwStepMotorRunCount > 1)
        ptThis->hwStepMotorRunCount--;
    if(ptThis->chExposureTimeCount > 1)
        ptThis->chExposureTimeCount--;
    if(ptThis->chCaptureWaitCount > 1)
        ptThis->chCaptureWaitCount--;

    if(ptThis->chExposureDelayCount > 1)
        ptThis->chExposureDelayCount--;
    if(ptThis->chWaitReaptcount > 1)
        ptThis->chWaitReaptcount--;
    
    for(chStepMotorNumOffset = 0; chStepMotorNumOffset < 3; chStepMotorNumOffset++)
    {
        //captureimg_StepMotorWork(ptThis, chStepMotorNumOffset, g_chStepMotorFlag[ptThis->chRunTimeCount]);
        captureimg_StepMotorWork(ptThis, chStepMotorNumOffset, g_chStepMotorFlag[0]);
        captureimg_InclusiveWork(ptThis, chStepMotorNumOffset);
        
        if(ptThis->hwStepMotorRunAloneCount[chStepMotorNumOffset] > 1)
            ptThis->hwStepMotorRunAloneCount[chStepMotorNumOffset]--;
        else if(ptThis->hwStepMotorRunAloneCount[chStepMotorNumOffset])
        {
            captureimg_StepMotorStop(ptThis, chStepMotorNumOffset);
            ptThis->hwStepMotorRunAloneCount[chStepMotorNumOffset] = 0;
            if(ptThis->chStepMotorMaxTime == chStepMotorNumOffset)
            {
                GLOG_PRINTF("Motor run alone ok");
                gbase_EventPost(CORRESPONDENT, INCLUSIVE_Event_UpdatePosition);
            }
                
        }
    }
    for(uint8_t chOffset=0;chOffset<5;chOffset++)
    {
        captureimg_TriggerWork(ptThis, chOffset);
    }
    
    
    return wRet;
}

/**
 * Function: captureimg_Init
 * ----------------------------
 * This function initializes an captureimg object. It converts the given addresses to pointers, 
 * checks if the pointers are not NULL, copies the configuration members to the object, initializes 
 * the hardware, and registers the object in the GMSI list.
 *
 * Parameters: 
 * wObjectAddr: The address of the captureimg object.
 * wObjectCfgAddr: The address of the captureimg configuration object.
 *
 * Returns: 
 * GMSI_SUCCESS if the function runs successfully, GMSI_EAGAIN if the object cannot be registered 
 * in the GMSI list, or GMSI_FAILURE if any of the pointers is NULL.
 */
int captureimg_Init(uintptr_t wObjectAddr, uintptr_t wObjectCfgAddr)
{
    // Convert the given addresses to pointers
    captureimg_t *ptThis = (captureimg_t *)wObjectAddr;
    captureimg_cfg_t *ptCfg = (captureimg_cfg_t *)wObjectCfgAddr;

    // Check if the pointers are not NULL
    if (ptThis == NULL || ptCfg == NULL) {
        GLOG_PRINTF("Error: ptThis or ptCfg is NULL.");
        return GMSI_EFAIL;
    }

    /* Copy the configuration members to the object */

    /* Initialize the hardware */
    captureimg_StepMotorStop(ptThis, 0);
    captureimg_StepMotorStop(ptThis, 1);
    captureimg_StepMotorStop(ptThis, 2);

    // Register the object in the GMSI list
    ptThis->ptBase = &s_tCaptureimgBase;
    if (ptThis->ptBase == NULL) {
        return GMSI_EAGAIN;
    } else {
        s_tCaptureimgBaseCfg.wParent = wObjectAddr;
        return gbase_Init(ptThis->ptBase, &s_tCaptureimgBaseCfg);
    }
}